﻿using System;
using System.Collections.Generic;
using System.Windows;

namespace Microsoft.Popfly.GameCreator.GameEngine.Behaviors
{
    static class MoveHelpers
    {
        public static void CalculateMoveTowardUnitVector(double startX, double startY, double endX, double endY, out double towardX, out double towardY)
        {
            MathUtils.Normalize(endX - startX, endY - startY, out towardX, out towardY);
        }

        public static double CalculateAccelerationFromMaxSpeed(double maxSpeed, double drag)
        {
            return maxSpeed * drag;
        }

        /// <summary>
        /// Finds the closest actor to the point defined by x, y
        /// </summary>
        /// <param name="scene">The scene the actor is in</param>
        /// <param name="actorName">The name of the actor we are finding the closest instance of.</param>
        /// <param name="x">The X value for the point to which we are finding the closest actor.</param>
        /// <param name="y">The Y value for the point to which we are finding the closest actor.</param>
        /// <returns></returns>
        public static Actor FindClosestActor(List<Actor> actors, double x, double y)
        {
            if (actors.Count == 0)
            {
                return null;
            }
            else
            {
                Actor retVal = actors[0];
                double minDistSq = MathUtils.DistanceSquared(x, y, retVal.X, retVal.Y);

                for (int i = 1; i < actors.Count; i++)
                {
                    double curDistSq = MathUtils.DistanceSquared(x, y, actors[i].GetCenterX(), actors[i].GetCenterY());
                    if (curDistSq < minDistSq)
                    {
                        minDistSq = curDistSq;
                        retVal = actors[i];
                    }
                }
                return retVal;
            }
        }

        internal static void CalculateOffsetToward(Point from, Point to, double offset, out double incrX, out double incrY)
        {
            double unitX, unitY;
            MoveHelpers.CalculateMoveTowardUnitVector(from.X, from.Y, to.X, to.Y, out unitX, out unitY);
            incrX = unitX * offset;
            incrY = unitY * offset;
        }

        internal static void CalculateOffsetToward(bool useAntiBounce, double elapsedTimeInSeconds, Point from, Point to, double offset, out double incrX, out double incrY)
        {
            if (useAntiBounce)
            {
                CalculateOffsetTowardAntiBounce(elapsedTimeInSeconds, from, to, offset, out incrX, out incrY);
            }
            else
            {
                CalculateOffsetToward(from, to, offset, out incrX, out incrY);
            }
        }

        internal static void CalculateOffsetTowardAntiBounce(double elapsedTimeInSeconds, Point from, Point to, double offset, out double incrX, out double incrY)
        {
            double xDist = Math.Abs(from.X - to.X);
            double yDist = Math.Abs(from.Y - to.Y);

            if (xDist < 0.5 && yDist < 0.5)
            {
                incrX = 0;
                incrY = 0;
                return;
            }

            double unitX, unitY;
            MoveHelpers.CalculateMoveTowardUnitVector(from.X, from.Y, to.X, to.Y, out unitX, out unitY);
            incrX = unitX * offset;
            incrY = unitY * offset;

            // TODO: we could take into account drag here.
            if (Math.Abs(incrX * elapsedTimeInSeconds) > xDist) // we're moving too far, so let's cap it
            {
                incrX = (to.X - from.X) / elapsedTimeInSeconds;
            }

            if (Math.Abs(incrY * elapsedTimeInSeconds) > yDist) // we're moving too far, so let's cap it
            {
                incrY = (to.Y - from.Y) / elapsedTimeInSeconds;
            }
        }



        internal static double MaxSpeedSingleDimensionMoveTowardAntiBounce(double elapsedTimeInSeconds, double from, double to, double maxSpeed, double drag)
        {
            double distance = Math.Abs(to - from);
            if (distance < 0.5)
                return 0;

            double max = CalculateSingleDimensionOffset(from, to, maxSpeed);
            double incrAccel = MoveHelpers.CalculateAccelerationFromMaxSpeed(max, drag);



            if (maxSpeed > 0 && Math.Abs(incrAccel * elapsedTimeInSeconds) > distance) // we're moving too far, so let's cap it
            {
                incrAccel = (to - from) / elapsedTimeInSeconds;
            }

            return incrAccel;
        }

        internal static double CalculateSingleDimensionOffsetAntiBounce(double elapsedTimeInSeconds, double from, double to, double offset)
        {
            double distance = Math.Abs(to - from);
            if (distance < 0.5)
                return 0;

            if (offset > 0) // we're moving toward the point so let's make sure we don't overshoot it
            {
                if (distance < Math.Abs(offset) * elapsedTimeInSeconds) // we'd move past the point so clamp it
                {
                    return CalculateSingleDimensionOffset(from, to, distance / elapsedTimeInSeconds);
                }
                else
                {
                    return CalculateSingleDimensionOffset(from, to, offset);
                }
            }
            else // we're moving away so just return offset
            {
                return CalculateSingleDimensionOffset(from, to, offset);
            }

        }

        internal static double CalculateSingleDimensionOffset(double from, double to, double offset)
        {
            if (from < to)
            {
                return offset;
            }
            else
                return -offset;
        }

        internal static void MaxSpeedMoveToward(bool useAntiBounce, double elapsedTimeInSeconds, Point from, Point to, double maxSpeed, double xDrag, double yDrag, out double incrXAccel, out double incrYAccel)
        {
            if (useAntiBounce)
            {
                MaxSpeedMoveTowardAntiBounce(elapsedTimeInSeconds, from, to, maxSpeed, xDrag, yDrag, out incrXAccel, out incrYAccel);
            }
            else
            {
                MaxSpeedMoveToward(from, to, maxSpeed, xDrag, yDrag, out incrXAccel, out incrYAccel);
            }
        }

        internal static void MaxSpeedMoveTowardAntiBounce(double elapsedTimeInSeconds, Point from, Point to, double maxSpeed, double xDrag, double yDrag, out double incrXAccel, out double incrYAccel)
        {
            double xDist = Math.Abs(from.X - to.X);
            double yDist = Math.Abs(from.Y - to.Y);

            if (xDist < 0.5 && yDist < 0.5)
            {
                incrXAccel = 0;
                incrYAccel = 0;
                return;
            }

            double maxX, maxY;
            CalculateOffsetToward(from, to, maxSpeed, out maxX, out maxY);
            incrXAccel = MoveHelpers.CalculateAccelerationFromMaxSpeed(maxX, xDrag);
            incrYAccel = MoveHelpers.CalculateAccelerationFromMaxSpeed(maxY, yDrag);

            // TODO: we could take into account drag here.
            if (Math.Abs(incrXAccel * elapsedTimeInSeconds) > xDist) // we're moving too far, so let's cap it
            {
                incrXAccel = (to.X - from.X) / elapsedTimeInSeconds;
            }

            if (Math.Abs(incrYAccel * elapsedTimeInSeconds) > yDist) // we're moving too far, so let's cap it
            {
                incrYAccel = (to.Y - from.Y) / elapsedTimeInSeconds;
            }
        }

        internal static void MaxSpeedMoveToward(Point from, Point to, double maxSpeed, double xDrag, double yDrag, out double incrXAccel, out double incrYAccel)
        {
            double maxX, maxY;
            CalculateOffsetToward(from, to, maxSpeed, out maxX, out maxY);
            incrXAccel = MoveHelpers.CalculateAccelerationFromMaxSpeed(maxX, xDrag);
            incrYAccel = MoveHelpers.CalculateAccelerationFromMaxSpeed(maxY, yDrag);
        }

        /// <summary>
        /// Find the direction to rotate using acceleration or velocity so that we are facing radiansoffset from a point.
        /// </summary>
        /// <param name="elapsedTimeInSeconds">The elapsed time in seconds this frame.</param>
        /// <param name="currentRotation">The current direction the actor is facing</param>
        /// <param name="from">The center of the current actor</param>
        /// <param name="to">The point to rotate with respect to</param>
        /// <param name="rotateOffset">How much to rotate</param>
        /// <param name="offsetFromFacingInRadians">The angle from to to be facing</param>
        internal static double RotateTowardAorV(double elapsedTimeInSeconds, double currentRotation, Point from, Point to, double rotateOffset, double offsetFromFacingInRadians)
        {
            double rotateExtent, moveAngle;
            RotateToward(currentRotation, from, to, rotateOffset, offsetFromFacingInRadians, out rotateExtent, out moveAngle);

            if (Math.Abs(rotateExtent) < 0.5) return 0;
            if (Math.Abs(rotateExtent) * elapsedTimeInSeconds > Math.Abs(moveAngle))
            {
                rotateExtent = moveAngle / elapsedTimeInSeconds;
            }
            return rotateExtent;
        }

        /// <summary>
        /// Find the direction to rotate using acceleration or velocity so that we are facing radiansoffset from a point.
        /// </summary>
        /// <param name="elapsedTimeInSeconds">The elapsed time in seconds this frame.</param>
        /// <param name="currentRotation">The current direction the actor is facing</param>
        /// <param name="from">The center of the current actor</param>
        /// <param name="to">The point to rotate with respect to</param>
        /// <param name="rotateOffset">How much to rotate</param>
        /// <param name="offsetFromFacingInRadians">The angle from to to be facing</param>
        internal static double RotateTowardMaxSpeed(double elapsedTimeInSeconds, double currentRotation, Point from, Point to, double maxSpeed, double rotationDrag, double offsetFromFacingInRadians)
        {
            double rotateExtent, moveAngle;
            RotateToward(currentRotation, from, to, maxSpeed, offsetFromFacingInRadians, out rotateExtent, out moveAngle);

            rotateExtent *= rotationDrag;
            if (Math.Abs(rotateExtent) < 0.5) return 0;
            if (Math.Abs(rotateExtent) * elapsedTimeInSeconds > Math.Abs(moveAngle))
            {
                rotateExtent = moveAngle / elapsedTimeInSeconds;
            }
            return rotateExtent;
        }

        private static void RotateToward(double currentRotation, Point from, Point to, double rotateOffset, double offsetFromFacingInRadians, out double rotateExtent, out double moveAngle)
        {
            double angleToFacing = MathUtils.RadiansToDegrees(offsetFromFacingInRadians + MathUtils.AngleToPointRadians(MathUtils.Subtract(to, from)) - (1.5 * Math.PI));

            moveAngle = angleToFacing - currentRotation;
            while (moveAngle < 0) moveAngle += 360.0; // make it positive
            moveAngle %= 360.0; // make it less than 360

            if (moveAngle <= 180)
            {
                rotateExtent = rotateOffset;
            }
            else
            {
                rotateExtent = -rotateOffset;
            }
        }

        internal static double RotateTowardRadiansMaxSpeed(double elapsedTimeInSeconds, double currentRotation, double maxSpeed, double rotationDrag, double angleToFaceInRadians)
        {
            double rotateExtent, moveAngle;
            RotateTowardRadians(currentRotation, maxSpeed, angleToFaceInRadians, out rotateExtent, out moveAngle);

            rotateExtent *= rotationDrag;
            if (Math.Abs(rotateExtent) < 0.5) return 0;
            if (Math.Abs(rotateExtent) * elapsedTimeInSeconds > Math.Abs(moveAngle))
            {
                rotateExtent = moveAngle / elapsedTimeInSeconds;
            }
            return rotateExtent;
        }

        internal static double RotateTowardRadiansAorV(double elapsedTimeInSeconds, double currentRotation, double rotateOffset, double angleToFaceInRadians)
        {
            double rotateExtent, moveAngle;
            RotateTowardRadians(currentRotation, rotateOffset, angleToFaceInRadians, out rotateExtent, out moveAngle);

            if (Math.Abs(rotateExtent) < 0.5) return 0;
            if (Math.Abs(rotateExtent) * elapsedTimeInSeconds > Math.Abs(moveAngle))
            {
                rotateExtent = moveAngle / elapsedTimeInSeconds;
            }
            return rotateExtent;
        }

        private static void RotateTowardRadians(double currentRotation, double rotateOffset, double angleToFaceInRadians, out double rotateExtent, out double moveAngle)
        {
            moveAngle = MathUtils.RadiansToDegrees(angleToFaceInRadians) - currentRotation;
            if (moveAngle < 0) moveAngle += 360.0; // make it positive
            moveAngle %= 360.0; // make it less than 360

            if (moveAngle <= 180)
            {
                rotateExtent = rotateOffset;
            }
            else
            {
                rotateExtent = -rotateOffset;
            }
        }
    }
}